#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "/usr/include/math.h"
#include "ssm_conversions.h"
#include "ssm_3.h"
#include "ssm.h"
#include "solpos/solpos00.h"

FILE *fd;

void printTitle(Race race, Simulation sim, Dynamic d, Weather weather, Car c)
{
	printf("The Wining Solar Car Model Spreadsheet\n");
	printf("    speed=%4.1f          sunrise=%7.4f     Peukert No.=%8.5f\n\
    	Weight (N)=%5.0f    Day Legnth=%7.4f  Battery C=%12.2f\n\
    	AC_d=%6.4f         Noon Angle=%5.2f\n\
    	C_rr=%8.4f       P_max=%8.0f\n\
    	Motor eff.=%6.2f\n\
    	Parasitic Loss=%6.2f\n\
    	rho=%12.6f\n\
    	time_start=%7.4f  time_stop=%7.4f\n",
	MPSTOKMPH*d.speed, STOH*weather.sun_rise, c.B_n,
	c.W, STOH*weather.sun_length, STOH*c.B_CPeukert,
	c.A*c.C_d, RADTODEG*weather.phi_n, 
	c.C_rr, c.P_arrayMax,
	c.M_d,
	c.P_p,
	weather.rho,
	STOH*race.time_start, STOH*race.time_stop);
}

void printStat(Race race, Simulation sim, Dynamic d, Weather weather)
{
	printf("%7.3f %7.3f %4.1f %7.1f %7.1f %7.1f %7.1f %7.1f %7.1f %7.1f %8.4f %8.4f\n",
        STOH*(sim.t+sim.dt), 
        d.distance, 
        // MPSTOKMPH*d.speed,
        RADTODEG*weather.phi,
        d.P_array,
        d.P_a, 
        d.P_rr, 
        d.P_p, 
        d.E_drive/sim.dt,
        d.E_battery/sim.dt,
        STOH * d.B_capacity,
        d.DB_charge, 
        d.B_charge);
	//printf("DEBUG: sim.dt %f\n", sim.dt);
}

int initializeSimulation(double speed, Race *race, Simulation *sim, Dynamic *d, Car *c, Weather *weather)
{
	d->speed = KMPHTOMPS * speed;
	sim->imax = (race->time_stop - race->time_start + 0.5)/sim->dt;
	c->B_CPeukert = HTOS*4500.0 * pow(1500.0, c->B_n -1.0);
	return 0;
}

int modelSolpos(Simulation sim, Weather *weather, Leg leg)
{
	struct posdata pd, *pdat;
	long retval;
	int hour, min, sec;
	pdat = &pd;
	S_init(pdat);

   	pdat->longitude = leg.longitude; //could be realtime
   	pdat->latitude  = leg.latitude;  //could be realtime
   	pdat->timezone  =  -5.0;  //eastern time zone 
   	pdat->year      = 2006;    
   	pdat->daynum    =  203;
	//converting sim->t (present time) from seconds to hms
	hour = sim.t / 3600;
	min = (sim.t - (hour * 3600)) / 60;
	sec = sim.t - (hour * 3600) - (min * 60);
	//printf("hours = %d\n", hour);
	//printf("min = %d\n", min);
	
   	pdat->hour      = hour;
   	pdat->minute    = min;
   	pdat->second    = sec;
  	pdat->temp      = 27.0; //deg C //to be updated in realtime
  	pdat->press     = 1006.0; //to be updated in realtime
  	//pdat->tilt      = pdat->latitude;  /* Tilted at latitude */
  	//pdat->aspect    = 135.0;       /* 135 deg. = SE */

	retval = S_solpos (pdat);  /* S_solpos function call */
    	S_decode(retval, pdat);    /* ALWAYS look at the return code! */

	weather->phi = DEGTORAD*pdat->elevref;
	//printf("DEBUG:SOLPOS| phi = %f\n", weather->phi);

	return 0;
}

//int modelControl(Race *race, Simulation *sim, Dynamic *d, Car *c, Weather *weather)
int modelControl(Dynamic *d)
{
     	d->distance += d->ddistance;
	return 0;
}

//int modelFPE(Race *race, Simulation *sim, Dynamic *d, Car *c, Weather *weather)
int modelAero(Simulation sim, Dynamic *d, Car c, Weather weather)
{
	/* find airspeed */
	double wswrtc; //wind speed with respect to car direction
	wswrtc = weather.winds * cos((((M_PI/2)-weather.windd))-(((M_PI/2)-d->heading)));
	d->airspeed = d->speed - wswrtc;
     	d->D_a = 0.5 * weather.rho * d->airspeed * d->airspeed * c.A * c.C_d; // (N)
     	d->P_a = d->D_a * d->airspeed;                             // (W = N M/S) 
     	d->E_a = d->P_a * sim.dt;                                  // (J = W S)
	return 0;
	/* notes:
	 * ~do we want to account for side loading on the bearings due to cross winds?
	 * if so this is the place to start.
	 * ~do we want to model the "sail boat" effect? if so this is also the place to start
	 * ~I think both are more effort than they are worth, if someone researches side 
	 * loading, and models it for me, ill put it in, otherwise it will remain a note.
	 */
}

//int modelRollingR(Race *race, Simulation *sim, Dynamic *d, Car *c, Weather *weather)
int modelRollingR(Simulation sim, Dynamic *d, Car c)
{
     	d->D_rr = c.C_rr * (  1.0 + MPSTOKMPH*d->speed/161.0) * c.W ;
     	d->P_rr = d->D_rr * d->speed;
     	d->E_rr = d->P_rr * sim.dt;
	return 0;
}

//int modelArray(Race *race, Simulation *sim, Dynamic *d, Car *c, Weather *weather)
int modelArray(Simulation sim, Dynamic *d, Car c, Weather *weather)
{
     	weather->theta = weather->phi; 
     	d->P_array = c.P_arrayMax * cos(weather->theta) * pow(cos(weather->phi), 0.3) * (1.0 -weather->CloudCover);
     	d->E_array = d->P_array * sim.dt; // Energy (W S)
	return 0;
	/* notes:
	 * the angle of the sun is already taken into account, but it assumes that the array is level
	 * ~a mini model of the array should be created
	 * ~should moving fronts be modeled or just guessed in the route?
	 * ~can more of the nrel stuff be used here?
	 * ~should the spectrum that reaches the earth compared to the time of day be
	 * 	evaluated and added to the model? ie. for tripple junction cells
	 */
}

//int modelParisitic(Race *race, Simulation *sim, Dynamic *d, Car *c, Weather *weather)
int modelParisitic(Simulation sim, Dynamic *d, Car c)
{
     	// Parisitic Losses
     	d->P_p = c.P_p; 
     	d->E_p = d->P_p * sim.dt; 
	return 0;
}

int modelKE(Simulation *sim, Dynamic *d, Car *c, Weather *weather, Route route, int loopPos, double speedOld, double speedNew)
{
	double time, distance;
	double oldKE, newKE;
	double avgSpeed;
	double orgDt;
	double orgDdist;
	oldKE = 0.5 * c->M * pow(speedOld, 2);
	newKE = 0.5 * c->M * pow(speedNew, 2);
	d->E_k = newKE - oldKE;
	orgDt = sim->dt;
	orgDdist = d->ddistance;
	if(d->E_k > 0)//acceleration required
	{
		time = (speedNew - speedOld)/c->accR;
		distance = 0.5 * c->accR * time * time;
		d->B_capacity -= d->E_k;
		sim->t += time;
		//d->distance += (UTOKU* distance);
		avgSpeed = fabs(distance/time);	
		printf("distance %f\n", UTOKU * distance);
		if(distance != 0)
		{
			d->ddistance = UTOKU * distance;
			sim->dt = time;
			model(sim, d, c, weather, route, loopPos);
			sim->dt = orgDt;
			d->ddistance = orgDdist;
		}
	}
	else//deceleration found
	{
		time = (speedNew - speedOld)/-c->decelR;
		distance = 0.5 * -c->decelR * pow(time, 2);
		d->B_capacity += d->E_k; //temporary model
		sim->t += time;
		d->distance += (UTOKU * distance);
		avgSpeed = fabs(distance/time);
		if(distance != 0)
		{
			d->ddistance = UTOKU * distance;
			sim->dt = time;
			model(sim, d, c, weather, route, loopPos);
			sim->dt = orgDt;
			d->ddistance = orgDdist;
		}

	}
	return 0;
}

int modelPE(Simulation sim, Dynamic *d, Car c, Leg legStart, Leg legFinish)
{
	double heightChangeLeg, heightChangeStep;
	double angle;
	heightChangeLeg = (legFinish.altitude - legStart.altitude);
	if(heightChangeLeg == 0)
		angle = 0;
	else
	{
		//printf("leg height %f\n", heightChangeLeg);
		//printf("distance   %f\n", legStart.distance);
		angle = asin(legStart.distance/heightChangeLeg);
		//printf("angle      %f\n", RADTODEG * angle);
	}
	heightChangeStep = d->ddistance * sin(angle);
	//printf("angle %f\n", RADTODEG * angle);
	d->E_g = c.M * (9.80) * heightChangeStep;
	d->P_g = d->E_g / sim.dt;
	
	return 0;
}

//int modelEnergy(Race *race, Simulation *sim, Dynamic *d, Car *c, Weather *weather)
int modelEnergy(Simulation sim, Dynamic *d, Car c)
{
  	// Compute Energy Needed  - consider motor eff. and no regen
  	// Note parasitic losses do not go thorugh the motor.
  	d->P_drive = (d->P_a + d->P_rr)/c.M_d + d->P_p + d->P_g;
  	d->E_drive = d->P_drive * sim.dt;
	return 0;
}

int modelMotor(Dynamic *d, Car c)
{	
	// Motor and controller drive/regen losses (should check motor limits)
  	// These effifiencies are "best" and are only true at near full rpms 
  	// and 80% max current. Looking at the torque curves from CISCIO a
  	// better model can be produced.
	//  
	//if( energy >= 0.0) 
	//{ // drive, energy taken from battery
		//energy /= c.M_d; //is this energy drive energy? i think so, should it be power?
		
	//} 
	//else 
	//{ // regen, energy returned to battery
		//energy *= c.M_r;//same questions as above
		
	//}
	return 0;
}

//int modelBatteryCharge(Race *race, Simulation *sim, Dynamic *d, Car *c, Weather *weather)
int modelBatteryCharge(Simulation sim, Dynamic *d, Car c)
{
  	d->P_battery = d->P_drive - d->P_array;
  	d->E_battery = d->P_battery * sim.dt;
  	if(d->P_battery >= 0.0) 
	{ // Take energy from battery
  		if(d->P_battery > c.B_CRate / 4.0) 
		{
      			d->B_capacity = c.B_CPeukert / pow(d->P_battery, c.B_n-1.0);
   		} 
		else 
		{
      			d->B_capacity = c.B_CPeukert / pow(c.B_CRate/12.0, c.B_n-1.0);
    		}
    		d->DB_charge = d->P_battery*sim.dt/d->B_capacity;
    		d->B_charge += d->DB_charge;
  	} 
	else //car is in regen or is charging
	{            
		d->B_capacity -= d->P_battery * c.B_CHeff;
		d->DB_charge = d->P_battery*sim.dt/d->B_capacity;
		d->B_charge -= d->DB_charge;
		/* notes:
		 * the book has no good model for battery charging,
		 * the charge model may have to be devloped on a test bench
		 * with the actual battery pack, for now i will use a
		 * charge efficiency, which the book says for li-poly
		 * batterys is around %99
		 */
  	}
	return 0;
	/* more notes:
	 * ~should we account for efficiency changes due to temp? how much does 
	 * the effiency change? someone needs to make a battery model!
	 */
}

int initializeLeg(Race *race, Simulation *sim, Dynamic *d, Car *c, Weather *weather, Leg *leg, Route *route, int loopPos)
{
	d->speed = KMPHTOMPS * leg->speed_limit;
	sim->imax = (HTOS * (leg->distance/leg->speed_limit) + 0.5)/sim->dt;
	c->B_CPeukert = HTOS*4500.0 * pow(1500.0, c->B_n -1.0);
	d->ddistance = leg->distance/sim->imax;
	/* initialize stuff that was in the body */
	sim->speedNew = d->speed;
	sim->tDist += route->leg[loopPos].distance;
	weather->winds = leg->windspeed;
	weather->windd = leg->windheading *DEGTORAD;
	weather->CloudCover = leg->shade;

	modelKE(sim, d, c, weather, *route, loopPos, sim->speedOld, sim->speedNew);//model the change of KE from the previous leg to current leg
	if(route->leg[loopPos].stops == 1)
	{
		//determine distance and time for breaking, subtract distance and time from
		//simulation, and model energy loss at the end of the simulation.
		printf("DEBUG: finding breaking distance and time\n");
		sim->Btime = (sim->speedNew - sim->speedOld)/-c->decelR;
		sim->Bdistance = 0.5 * c->decelR * pow(sim->Btime, 2);
	}
	findHeading(d, route->leg[loopPos], route->leg[loopPos+1]);

	return 0;
}

int routeLoop(Race *race, Simulation *sim, Dynamic *d, Car *c, Weather *weather, Route *route)
{
	int iloop, oloop,x;

	/*here we check to make sure that the speed you are running at does 
	  not exceed the speed limit. If it does, change speed_limit under
	  the leg struct to reflect this
	*/

	for(x=0;x<=route->nleg;x++)
	{
		if(route->leg[x].speed>route->leg[x].speed_limit)
			route->leg[x].speed_limit=route->leg[x].speed;

	}

	printf("rho = %f\n", weather->rho);
	printf("c.C_d = %f\n", c->C_d);
	sim->t = race->time_start;
	d->distance = 0;
	sim->speedOld = 0.0;
	sim->tDist = 0.0;
	for(oloop = 0; oloop <= route->nleg - 1; oloop++)
	{
		printf("\nsimulating leg %d\n", oloop);

		//initialize leg
		initializeLeg(race, sim, d, c, weather, &route->leg[oloop], route, oloop);
		//simulate leg
		printf("wind speed and direction %f, %f\n", weather->winds, weather->windd);
		for(iloop = 0; iloop <= sim->imax - 1; iloop++)
		{
			model(sim, d, c, weather, *route, oloop);
			
			if(iloop == 0)
			{//print header
    				printHeader();
			}
			//printStat(*race, *sim, *d, *weather);
			logData(*d, *sim);
			sim->t += sim->dt;
	  		if(d->B_charge >= route->targetBatt) 
  			{ // Battery kaput
				printf("battery past limit\n Simulation ending\n");
    				return(1);
  			}
			if(sim->t >= route->targetTime)
			{ //target time exceeded
				printf("time limit exceeded\nSimulation ending\n");
				return(1);
			}
			if(d->distance+d->ddistance > (sim->tDist - UTOKU * sim->Bdistance))
			{//leg ended in the middle of a slice, should it be d->distance+d->distance, 
				double orgDt;
				orgDt = sim->dt;
				//to check if the next leg will end? i think so
				double oversim = d->distance + d->ddistance - sim->tDist - UTOKU * sim->Bdistance;

				/* set new distance and time settings... */
				d->ddistance = d->ddistance - oversim - UTOKU * sim->Bdistance;
				//printf("ddistance %f\n", d->ddistance);
				sim->dt = d->distance / d->speed;
				//run sim
				
				model(sim, d, c, weather, *route, oloop);

				printStat(*race, *sim, *d, *weather);
				logData(*d, *sim);
				sim->dt = orgDt; //reset dt
				break;//do not simulate past the end
				printStat(*race, *sim, *d, *weather);
			}

		}
		if(route->leg[oloop].stops == 1)
		{
			//printf("DEBUG LOOP: setting speedNew to 0.0\n");
			//modelBreaking();//if a stop is present, model breaking
			
			d->E_k = 0.5 * c->M * pow(d->speed, 2);

			sim->t += sim->Btime + sim->stoptime;
			d->distance += (UTOKU* sim->Bdistance);
			d->B_capacity += d->E_k;
			if(route->leg[oloop].suntime > 0)
			{//sun the array
				printf("sun the array\n");
				d->P_array = c->P_arrayMax * cos(weather->theta) * pow(cos(weather->phi), 0.3) * (1.0 -weather->CloudCover);
				d->B_capacity += d->P_array * c->B_CHeff;
				d->DB_charge = d->P_array*route->leg[oloop].suntime/d->B_capacity;
				d->B_charge -= d->DB_charge;


			}

			sim->speedNew = 0.0;
		}
		sim->speedOld = sim->speedNew;
	}
	return 0;
}

int findHeading(Dynamic *d, Leg legStart, Leg legFinish)
{
	d->heading = atan2(sin(legFinish.longitude-legStart.longitude)*
			cos(legFinish.latitude),cos(legStart.latitude)*sin(legFinish.latitude)-
			sin(legStart.latitude)*cos(legFinish.latitude)*cos(legFinish.longitude-
				legStart.longitude)) / 2*M_PI;
	return 0;
}

int model(Simulation *sim, Dynamic *d, Car *c, Weather *weather, Route route, int loopPos)
{
	modelSolpos	  (*sim, weather, route.leg[loopPos]);
	modelControl      (d);
	modelAero         (*sim, d, *c, *weather);
	modelRollingR     (*sim, d, *c);
	modelArray        (*sim, d, *c, weather);
	modelParisitic    (*sim, d, *c);
	modelPE           (*sim, d, *c, route.leg[loopPos], route.leg[loopPos+1]);
	modelEnergy       (*sim, d, *c);
	modelMotor	  (d, *c);
	modelBatteryCharge(*sim, d, *c);
	return 0;
}

void printHeader()
{
	printf("Time	dist    phi    P_array P_a     P_rr    P_p    P Total  P_bat  B_cap   DB_charge B_charge\n");
	printf("Period   	(DEG)  (W)     (W)     (W)     (W)    (W)      (W)    (W H)   \n");

}

int logData(Dynamic d, Simulation sim)
{
	fprintf(fd, " %f \t %f \n",d.distance, d.B_charge);
	//printf("-----%f\n", d.distance);
	return 0;
}

int plotIze()
{/*
	int source, dest;
	double val[4];
	int rv, i;
	source = fopen("data.log", "r");
	dest = fopen("plot.dat", "w+");
	for(i = 0; i < 3; i++)
	{
		while((rv = fscanf(source, "%lf %lf %lf %lf", &val[0], &val[1], &val[2], &val[3])) > 0)
		{
			if(i == 2)
			{ // logging speed
				val[3] *=3.6;
			}
			//printf("return val = %d\n", rv);
			fprintf(dest, "%f \t%f\n", val[0], val[i+1]);
		}
		fprintf(dest, "\n\n");
		fseek(source, 0, SEEK_SET);
	}
	fclose(source);
	fclose(dest);

	system("gnuplot plotting.plt -");
	
	return 0;
*/
	system("xmgrace -param param.par data.log waypoints.dat");

	return 0;
}
